initDHMM = function(States,Symbols,startProbs=NULL,transProbs=NULL,emisProbs=NULL,duration,durationProbs=NULL)
{
  nStates=length(States)
  nSymbols=length(Symbols)
  S=rep(1/nStates,nStates)
  T=array(1/(nStates-1), c(nStates, nStates)) - 1/(nStates-1)*diag(nStates)
  E=array(1/(nSymbols),c(nStates,nSymbols))
  names(S)=States
  dimnames(T)=list(from=States,to=States)
  dimnames(E)=list(from=States,to=Symbols)
  D=list()
  DP=list()
  for (i in 1:nStates)
  {
    len=length(duration[[i]])
    D[[States[i]]] = duration[[i]]
    DP[[States[i]]] = rep(1/len,len)
  }
  if(!is.null(startProbs)){S[]  = startProbs[]}
  if(!is.null(transProbs)){T[,] = transProbs[,]}
  if(!is.null(emisProbs)){E[,] = emisProbs[,]}
  return(list(States=States,Symbols=Symbols,startProbs=S,transProbs=T,emissionProbs=E,duration=D,durationProbs=DP))
}

simHMM = function(hmm, length)
{
  hmm$transProbs[is.na(hmm$transProbs)]       = 0
  hmm$emissionProbs[is.na(hmm$emissionProbs)] = 0
  nStates=length(hmm$States)
  nSymbols=length(hmm$Symbols)
  states   = c()
  emission = c()
  state = sample(hmm$States,1,prob=hmm$startProbs)
  d = sample(hmm$duration[[state]],1,prob=hmm$durationProbs[[state]])
  states = c(states, rep(state, d))
  for(i in 2:length)
  {
    lastState=states[length(states)]
    state  = sample(hmm$States, 1, prob=hmm$transProbs[lastState,])
    d = sample(hmm$duration[[state]],1,prob=hmm$durationProbs[[state]])
    states = c(states, rep(state, d))
  }
  for(i in 1:length(states))
  {
    emi = sample(hmm$Symbols, 1, prob=hmm$emissionProbs[states[i],])
    emission = c(emission, emi)
  }
  return(list(states=states,observation=emission))
}

forward = function(hmm, observation)
{
  hmm$transProbs[is.na(hmm$transProbs)]       = 0
  hmm$emissionProbs[is.na(hmm$emissionProbs)] = 0
  nObservations  = length(observation)
  nStates    = length(hmm$States)
  f          = array(NA,c(nStates,nObservations))
  dimnames(f)= list(states=hmm$States,index=1:nObservations)

  for (t in 1:nObservations)
  {
#    print(c("t=",t))
    for (state in hmm$States)
    {
#      print(c("s=",state))
      d_idx=1
      logSum = -Inf
      for (d in hmm$duration[[state]])
      {
#	print(c("d=",d, "d_idx=",d_idx))
	dp = hmm$durationProbs[[state]][d_idx]
	if (d == t)
	{
	  tmp = log(hmm$startProbs[state]*dp)+logSeqProb(hmm, state, observation[1:t]);
	  logSum = logAdd(logSum,tmp)
	}
	else if (d < t)
	{
	  seqProb = logSeqProb(hmm, state, observation[(t-d+1):t])
	  for (prevState in hmm$States) if (prevState != state)
	  {
	    tmp = f[prevState, t-d] + log(hmm$transProbs[prevState,state]*dp) + seqProb;
	    logSum = logAdd(logSum,tmp)
	  }
	}
	d_idx = d_idx+1;
      }
      f[state,t] = logSum
    }
  }
  return(f)
}

forwardS = function(hmm, observation, f)
{
  hmm$transProbs[is.na(hmm$transProbs)]       = 0
  hmm$emissionProbs[is.na(hmm$emissionProbs)] = 0
  nObservations  = length(observation)
  nStates    = length(hmm$States)
  fS          = array(NA,c(nStates,nObservations))
  dimnames(fS)= list(states=hmm$States,index=1:nObservations)

  for (t in 1:nObservations)
  {
#    print(c("t=",t))
    for (state in hmm$States)
    {
      logSum = -Inf
      for (prevState in hmm$States) if (prevState != state)
      {
	tmp = log(hmm$transProbs[prevState,state]) + f[prevState,t]
	logSum = logAdd(logSum, tmp)
      }
      fS[state,t] = logSum
    }
  }
  return(fS)
}

backward = function(hmm, observation)
{
  hmm$transProbs[is.na(hmm$transProbs)]       = 0
  hmm$emissionProbs[is.na(hmm$emissionProbs)] = 0
  nObservations  = length(observation)
  nStates    = length(hmm$States)
  b          = array(NA,c(nStates,nObservations))
  dimnames(b)= list(states=hmm$States,index=1:nObservations)

  for (state in hmm$States)
  {
    b[state,nObservations] = 0
  }

  for (t in (nObservations-1):1)
  {
    for (state in hmm$States)
    {
      logSum = -Inf
      for (nextState in hmm$States) if (nextState != state)
      {
	d_idx=1
	for (d in hmm$duration[[nextState]]) if (t+d <= nObservations)
	{
	  dp = hmm$durationProbs[[nextState]][d_idx]
	  d_idx = d_idx+1
	  seqProb = logSeqProb(hmm, nextState, observation[(t+1):(t+d)])
	  tmp = log(hmm$transProbs[state,nextState]*dp) + b[nextState,t+d] + seqProb
	  logSum = logAdd(logSum,tmp)
	}
      }
      b[state, t] = logSum
    }
  }
  return(b)
}

backwardS = function(hmm, observation, b)
{
  hmm$transProbs[is.na(hmm$transProbs)]       = 0
  hmm$emissionProbs[is.na(hmm$emissionProbs)] = 0
  nObservations  = length(observation)
  nStates    = length(hmm$States)
  bS          = array(NA,c(nStates,nObservations+1))
  dimnames(bS)= list(states=hmm$States,index=1:(nObservations+1))

  for (state in hmm$States)
    bS[state,nObservations+1] = 0
  for (t in (nObservations-1):0)
  {
    for (state in hmm$States)
    {
      logSum = -Inf
      d_idx=1
      for (d in hmm$duration[[state]]) if (t+d <= nObservations)
      {
	dp = hmm$durationProbs[[state]][d_idx]
	d_idx = d_idx+1
	seqProb = logSeqProb(hmm, state, observation[(t+1):(t+d)])
	tmp = log(dp) + b[state,t+d] + seqProb
	logSum = logAdd(logSum, tmp)
      }
      bS[state,t+1]=logSum
    }
  }
  return(bS)
}

check_fb = function(hmm, observation)
{
  nObservations  = length(observation)
  f = forward(hmm, observation)
  b = backward(hmm, observation)
  fStar = forwardS(hmm,observation,f)
  bStar = backwardS(hmm,observation, b)
 
  #print(f)
  #print(fStar)
  #print(b)
  print(bStar)

  fs = -Inf
  for (state in hmm$States)
  {
    tmp = f[state, nObservations]
    fs = logAdd(fs,tmp)
  }

  bs = -Inf
  for (state in hmm$States)
  {
    d_idx = 1
#    print(c("s=",state,"bs=",bs))
    for (d in hmm$duration[[state]]) if (d <= nObservations)
    {
      dp = hmm$durationProbs[[state]][d_idx]
      seqProb = logSeqProb(hmm, state, observation[1:d])
      tmp = log(hmm$startProbs[state]*dp)+seqProb+b[state, d]
      bs = logAdd(bs,tmp)
      d_idx = d_idx+1
    }
  }


  print(bs)
  print(fs)
  return(abs(exp(bs)-exp(fs)))
}

logSeqProb = function(hmm, state, observation)
{
  logSum=0;
  for (o in observation)
  {
    logSum = logSum + log(hmm$emissionProbs[state,o])
  }
  return(logSum);
}

logAdd = function(a,b)
{
  if (a==-Inf)
    return(b)
  else if (b==-Inf)
    return(a)
  else if (a < b)
    return(b+log(1+exp(a-b)))
  else
    return(a+log(1+exp(b-a)))
}

logSub = function(a,b)
{
  #print(c("a=",a,"b=",b))
  if (b==-Inf)
    return(a)
  else if (abs(a-b) < 1e-9)
    return(-Inf)
  else 
    return(a+log(1-exp(b-a)))
}

baumWelch = function(hmm, observation, maxIterations=100, delta=1E-9, pseudoCount=0)
{
	tempHmm = hmm
	tempHmm$transProbs[is.na(hmm$transProbs)]       = 0
	tempHmm$emissionProbs[is.na(hmm$emissionProbs)] = 0
	diff = c()
	prob = c()
	for(i in 1:maxIterations)
	{
		print(c("iter=", i))
		# Expectation Step (Calculate expected Transitions and Emissions)
		bw = baumWelchRecursion(tempHmm, observation)
		T  = bw$TransitionMatrix
		E  = bw$EmissionMatrix
		DP = bw$DurationProbs
		# Pseudocounts
		T[!is.na(hmm$transProbs)]    = T[!is.na(hmm$transProbs)]    + pseudoCount
		E[!is.na(hmm$emissionProbs)] = E[!is.na(hmm$emissionProbs)] + pseudoCount
		# Maximization Step (Maximise Log-Likelihood for Transitions and Emissions-Probabilities)
		T = (T/apply(T,1,sum))
		E = (E/apply(E,1,sum))
		for (state in hmm$States)
		  DP[[state]] = (DP[[state]]/sum(DP[[state]]))
		d = sqrt(sum((tempHmm$transProbs-T)^2)) + sqrt(sum((tempHmm$emissionProbs-E)^2))
		for (state in hmm$States)
		  d = d + sqrt(sum((tempHmm$durationProbs[[state]]-DP[[state]])^2))
		diff = c(diff, d)
		prob = c(prob, bw$prob)
		tempHmm$transProbs    = T
		tempHmm$emissionProbs = E
		tempHmm$durationProbs = DP
		if(d < delta)
		{
			break
		}
	}
	tempHmm$transProbs[is.na(hmm$transProbs)]       = NA
	tempHmm$emissionProbs[is.na(hmm$emissionProbs)] = NA
	return(list(hmm=tempHmm,difference=diff,logProb=prob))
}

baumWelchRecursion = function(hmm, observation)
{
  nObservations  = length(observation)
  T = hmm$transProbs
  E = hmm$emissionProbs
  DP = hmm$durationProbs

  f = forward(hmm, observation)
  b = backward(hmm, observation)
  fStar = forwardS(hmm,observation,f)
  bStar = backwardS(hmm,observation, b)
 
  #print(f)
  #print(fStar)
  #print(b)
  #print(bStar)

  seqProb = -Inf
  for (state in hmm$States)
  {
    tmp = f[state, nObservations]
    seqProb = logAdd(seqProb, tmp)
  }
  print(c("prob=", seqProb))

  #Duration Prob
  for (state in hmm$States)
  {
    d_idx = 1
    for (d in hmm$duration[[state]])
    {
      dp = hmm$durationProbs[[state]][d_idx]
      logSum = -Inf
      for (t in 1:nObservations) if (t+d <= nObservations)
      {
	tmp = fStar[state,t] + log(dp) + b[state,t+d] + logSeqProb(hmm, state, observation[(t+1):(t+d)])
	logSum = logAdd(logSum, tmp)
      }
      DP[[state]][d_idx] = exp(logSum-seqProb)
      d_idx = d_idx+1
    }
  }

  #Transmission Prob
  for (state in hmm$States)
    for (nextState in hmm$States) if (state != nextState)
    {
      logSum = -Inf
      for (t in 1:nObservations)
      {
	tmp = f[state,t] + log(hmm$transProb[state,nextState]) + bStar[state, t+1]
	logSum = logAdd(logSum, tmp)
      }
      T[state, nextState] = exp(logSum-seqProb)
    }

  # Emission Prob
  for (state in hmm$States)
    for (symbol in hmm$Symbols)
    {
      s = -Inf
      sum1 = log(hmm$startProbs[state])+bStar[state,1]
      sum2 = -Inf
      for (t in 1:nObservations)
      {
	if (observation[t] == symbol)
	{
	  s = logAdd(s, sum1)
	  s = logSub(s, sum2)
	}
	tmp = fStar[state,t]+bStar[state,t+1]
	sum1 = logAdd(sum1, tmp)
	tmp = f[state,t]+b[state,t]
	sum2 = logAdd(sum2, tmp)
      }
      E[state,symbol] = exp(s-seqProb)
    }
  return(list(TransitionMatrix=T,EmissionMatrix=E,DurationProbs=DP,prob=seqProb))
}
